/*!
  @file motion_estimator.cpp
  @copyright 2013 Kubota Lab. All rights resereved.
*/

#include "mrover_vo/motion_estimator.h"

namespace vo {

void MotionEstimator::compute() {
  if (!initCompute()) {
    ROS_ERROR("error in motion estimation");
    matches_->clear();
    return;
  }

  trackFeature();
  ROS_INFO("track %d", matches_->size());

  solver_->setInputFeature(input_);
  solver_->setTargetFeature(target_);
  solver_->setInputMatch(matches_);
  solver_->compute(pose_);
  acc_pose_ = acc_pose_ * pose_;


  ++counter_;
  if (counter_ == MAX_COUNT) {
    boost::shared_ptr<Feature::Matches> key_matches(new Feature::Matches);
    ROS_INFO("performing key frame adjustment(%d)", counter_);
    tracker_->setInputFeature(key_input_->kp_vec[0], key_input_->desc_vec[0]);
    tracker_->setTargetFeature(target_->kp_vec[0], target_->desc_vec[0]);
    tracker_->compute(key_matches);

    cv::Mat adjusted_pose = acc_pose_.clone();
    adjuster_->setInputFeature(key_input_);
    adjuster_->setTargetFeature(target_);
    adjuster_->setInputMatch(key_matches);
    adjuster_->enableInitialGuess();
    adjuster_->computeLM(adjusted_pose);

    //ROS_INFO_STREAM("Accumulated pose" << std::endl << acc_pose_);
    //ROS_INFO_STREAM("Recalculated pose" << std::endl << pose_);
    pose_ = pose_ * acc_pose_.inv() * adjusted_pose;
    //ROS_INFO_STREAM("Diff" << std::endl << pose_);
    
    cv::Mat t = pose_(cv::Rect(3, 0, 1, 3));
    ROS_INFO_STREAM("Accumulated error: " << cv::norm(t));

    resetKeyFrame(target_);
  }
}

void MotionEstimator::trackFeature() {
  tracker_->setInputFeature(input_->kp_vec[0], input_->desc_vec[0]);
  tracker_->setTargetFeature(target_->kp_vec[0], target_->desc_vec[0]);
  tracker_->compute(matches_);
}

}  // namespace vo
